/*
 * OREKIT-X
 * Copyright 2002-2008 CS Communication & Systemes
 * 
 * Parts of this software package have been licensed to CS
 * Communication & Systemes (CS) under one or more contributor license
 * agreements.  See the NOTICE file distributed with this work for
 * additional information.
 *  
 * This is an experimental copy of OREKIT from www.orekit.org.
 * Please use the original OREKIT from orekit.org for normal work
 * unrelated to this research project.
 * 
 * Licensed under the Apache License, Version 2.0 (the "License"); you
 * may not use this file except in compliance with the License.  You
 * may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
 * implied.  See the License for the specific language governing
 * permissions and limitations under the License.
 */
package ore.orbits;

import ore.Frame;
import ore.PositionVelocity;
import ore.errors.OrekitException;
import ore.time.AbsoluteDate;

import org.apache.commons.math.geometry.Vector3D;

/**
 * The parameters used internally are the classical keplerian elements:
 *   <pre>
 *     a
 *     e
 *     i
 *     &omega;
 *     &Omega;
 *     v
 *   </pre>
 * where &omega; stands for the Perigee Argument, &Omega; stands for the
 * Right Ascension of the Ascending Node and v stands for the true anomaly.
 * 
 * When orbit is either equatorial or circular, some keplerian elements
 * (more precisely &omega; and &Omega;) become ambiguous so this class should not
 * be used for such orbits. For this reason, {@link EquinoctialOrbit equinoctial
 * orbits} is the recommended way to represent orbits.
 * 
 * The instance of this class is immutable.
 * 
 * @see    ore.Orbit
 * @see    CircularOrbit
 * @see    CartesianOrbit
 * @see    EquinoctialOrbit
 * 
 * @author Luc Maisonobe
 * @author Guylaine Prat
 * @author Fabien Maussion
 * @author V&eacute;ronique Pommier-Maurussane
 */
public class KeplerianOrbit
    extends ore.Orbit
{

    public static final int MEAN_ANOMALY = 0;
    public static final int ECCENTRIC_ANOMALY = 1;
    public static final int TRUE_ANOMALY = 2;

    /** 
     * Eccentricity threshold for near circular orbits.
     *  If (e &lt; E_CIRC), then the orbit is considered circular.
     */
    public static final double E_CIRC = 1.e-10;


    /** 
     * Semi-major axis (m).
     */
    public final double a;
    /** 
     * Eccentricity.
     */
    public final double e;
    /** 
     * Inclination (rad).
     */
    public final double i;
    /** 
     * Perigee Argument (rad).
     */
    public final double pa;
    /** 
     * Right Ascension of Ascending Node (rad).
     */
    public final double raan;
    /** 
     * True anomaly (rad). 
     */
    public final double v;

    /** 
     * @param a  semi-major axis (m)
     * @param e eccentricity
     * @param i inclination (rad)
     * @param pa perigee argument (&omega;, rad)
     * @param raan right ascension of ascending node (&Omega;, rad)
     * @param anomaly mean, eccentric or true anomaly (rad)
     * @param type type of anomaly, must be one of {@link #MEAN_ANOMALY},
     * {@link #ECCENTRIC_ANOMALY} or  {@link #TRUE_ANOMALY}
     * @param frame the frame in which the parameters are defined
     * @param date date of the orbital parameters
     * @param mu central attraction coefficient (m<sup>3</sup>/s<sup>2</sup>)
     * @exception IllegalArgumentException if the longitude argument type is not
     * one of {@link #MEAN_ANOMALY}, {@link #ECCENTRIC_ANOMALY}
     * or {@link #TRUE_ANOMALY}
     * @see #MEAN_ANOMALY
     * @see #ECCENTRIC_ANOMALY
     * @see #TRUE_ANOMALY
     */
    public KeplerianOrbit(final double a, final double e, final double i,
                          final double pa, final double raan,
                          final double anomaly, final int type,
                          final Frame frame, final AbsoluteDate date, final double mu)
        throws IllegalArgumentException
    {
        super(frame, date, mu);
        this.a    =    a;
        this.e    =    e;
        this.i    =    i;
        this.pa   =   pa;
        this.raan = raan;

        switch (type) {
        case MEAN_ANOMALY :
            this.v = computeMeanAnomaly(anomaly);
            break;
        case ECCENTRIC_ANOMALY :
            this.v = computeEccentricAnomaly(anomaly);
            break;
        case TRUE_ANOMALY :
            this.v = anomaly;
            break;
        default :
            throw OrekitException.createIllegalArgumentException(
                  "angle type not supported, supported angles: {0}, {1} and {2}",
                  "MEAN_ANOMALY", "ECCENTRIC_ANOMALY", "TRUE_ANOMALY");

        }
    }
    /** 
     * Cartesian parameters.
     * @param pvCoordinates the PositionVelocity of the satellite
     * @param frame the frame in which are defined the {@link PositionVelocity}
     * @param date date of the orbital parameters
     * @param mu central attraction coefficient (m<sup>3</sup>/s<sup>2</sup>)
     */
    public KeplerianOrbit(final PositionVelocity pvCoordinates,
                          final Frame frame, final AbsoluteDate date, final double mu)
    {
        super(pvCoordinates, frame, date, mu);
        /*
         * Compute semi-major axis
         */
        final Vector3D pvP = pvCoordinates.getPosition();
        final Vector3D pvV = pvCoordinates.getVelocity();
        final double r = pvP.getNorm();
        final double V2 = Vector3D.dotProduct(pvV, pvV);
        final double rV2OnMu = r * V2 / mu;
        this.a = r / (2 - rV2OnMu);
        /*
         * Compute eccentricity
         */
        final double muA = mu * a;
        final double eSE = Vector3D.dotProduct(pvP, pvV) / Math.sqrt(muA);
        final double eCE = rV2OnMu - 1;
        this.e = Math.sqrt(eSE * eSE + eCE * eCE);
        /*
         * Compute inclination
         */
        final Vector3D momentum =
            Vector3D.crossProduct(pvP, pvV);
        final double m2 = Vector3D.dotProduct(momentum, momentum);
        this.i = Vector3D.angle(momentum, Vector3D.PLUS_K);
        /*
         * Compute right ascension of ascending node
         */
        final Vector3D node = Vector3D.crossProduct(Vector3D.PLUS_K, momentum);
        final double n2 = Vector3D.dotProduct(node, node);
        /*
         * The following comparison with 0 IS REALLY numerically justified and stable
         */
        this.raan = (n2 == 0) ? 0 : Math.atan2(node.getY(), node.getX());
        /*
         * Compute true anomaly
         */
        if (e < E_CIRC)
            this.v = 0;
        else {
            final double E = Math.atan2(eSE, eCE);
            final double k = 1 / (1 + Math.sqrt(m2 / muA));
            this.v = E + 2 * Math.atan(k * eSE / (1 - k * eCE));
        }
        /*
         * Compute perigee argument
         */
        final double cosRaan = Math.cos(raan);
        final double sinRaan = Math.sin(raan);
        final double px = ((cosRaan * pvP.getX()) +
                           (sinRaan * pvP.getY()));
        final double py = ((Math.cos(i) * (cosRaan * pvP.getY() - sinRaan * pvP.getX())) +
                           (Math.sin(i) * pvP.getZ()));

        this.pa = (Math.atan2(py, px) - v);
    }
    /** 
     * @param op orbital parameters to copy
     */
    public KeplerianOrbit(ore.Orbit op) {
        super(op);
        this.a    = op.getA();
        this.e    = op.getE();
        this.i    = op.getI();
        this.raan = Math.atan2(op.getHy(), op.getHx());
        this.pa   = (Math.atan2(op.getEquinoctialEy(), op.getEquinoctialEx()) - this.raan);
        this.v    = (op.getLv() - (this.pa + this.raan));
    }


    /** 
     * @return semi-major axis (m)
     */
    public double getA() {
        return this.a;
    }
    /** 
     * @return eccentricity
     */
    public double getE() {
        return this.e;
    }
    /** 
     * @return Inclination (rad)
     */
    public double getI() {
        return this.i;
    }
    /** 
     * @return perigee argument (rad)
     */
    public double getPerigeeArgument() {
        return this.pa;
    }
    /** 
     * @return Right ascension of the ascending node (rad)
     */
    public double getRightAscensionOfAscendingNode() {
        return this.raan;
    }
    /** 
     * @return First component of the eccentricity vector
     */
    public double getEquinoctialEx() {
        return (this.e * Math.cos(this.pa + this.raan));
    }
    /** 
     * @return second component of the eccentricity vector
     */
    public double getEquinoctialEy() {
        return (this.e * 
                Math.sin(this.pa + this.raan));
    }
    /** 
     * @return First component of the inclination vector.
     */
    public double getHx() {
        return (Math.cos(this.raan) * 
                Math.tan(this.i / 2.0));
    }
    /** 
     * @return Second component of the inclination vector.
     */
    public double getHy() {
        return (Math.sin(this.raan) * 
                Math.tan(this.i / 2.0));
    }
    /** 
     * @return True latitude argument (rad)
     */
    public double getLv() {
        return (this.pa + this.raan + this.v);
    }
    /** 
     * @return Eccentric latitude argument.(rad)
     */
    public double getLE() {
        return this.pa + this.raan + this.getEccentricAnomaly();
    }
    /** 
     * @return Mean latitude argument.(rad)
     */
    public double getLM() {
        return this.pa + this.raan + this.getMeanAnomaly();
    }
    /** 
     * @return True anomaly (rad)
     */
    public double getTrueAnomaly() {
        return this.v;
    }
    /** 
     * @return Eccentric anomaly (rad)
     */
    public double getEccentricAnomaly() {
        final double beta = (this.e /
                             (1 + Math.sqrt((1 - this.e) *
                                            (1 + this.e))));
        return (this.v -
                2.0 * Math.atan(beta * Math.sin(this.v) /
                                (1 + beta * Math.cos(this.v))));
    }
    /** 
     * @param E eccentric anomaly (rad)
     * @return v the true anomaly
     */
    private double computeEccentricAnomaly (double E) {
        double beta = (this.e / 
                       (1 + Math.sqrt((1 - this.e) *
                                      (1 + this.e))));
        return (E + 
                2.0 * Math.atan(beta * Math.sin(E) /
                                (1 - beta * Math.cos(E))));
    }
    /** 
     * @return Mean anomaly (rad)
     */
    public double getMeanAnomaly() {
        double E = this.getEccentricAnomaly();
        return (E -
                (this.e * Math.sin(E)));
    }
    /** 
     * @param M mean anomaly (rad)
     * @return v the true anomaly
     */
    private double computeMeanAnomaly (final double M) {
        /*
         * Resolution of Kepler equation for keplerian parameters
         */
        double E = M;
        double shift = 0.0;
        double EmM   = 0.0;
        int iter = 0;
        do {
            double f2 = (this.e * Math.sin(E));
            double f1 = (1.0 - this.e * Math.cos(E));
            double f0 = (EmM - f2);

            double f12 = 2 * f1;
            shift = f0 * f12 / (f1 * f12 - f0 * f2);

            EmM -= shift;
            E    = (M + EmM);
        }
        while ((++iter < 50) && (Math.abs(shift) > 1.0e-12));

        return this.computeEccentricAnomaly(E);
    }

    public String toString() {
        return new StringBuffer().append("keplerian parameters: ").append('{').
                                  append("a: ").append(a).
                                  append("; e: ").append(e).
                                  append("; i: ").append(Math.toDegrees(i)).
                                  append("; pa: ").append(Math.toDegrees(pa)).
                                  append("; raan: ").append(Math.toDegrees(raan)).
                                  append("; lv: ").append(Math.toDegrees(v)).
                                  append(";}").toString();
    }

}
